# -*- coding:utf-8 -*-

# a general message class
# an inportant point of view is that the computation method is for each satPosecial method.
from  builtins import range
from functools import reduce
import numpy as np
import math
import logging
import time
import comLibs
'''
    some msgs' names are based by *Hybrid cooperative positioning based on distributed belief propagation*, please refer to it. 
'''
np.set_printoptions(precision=17)

def normalMsgMerge(msgs):
    '''
        Merge multiple normal distributed messages (product of this pdfs)
        sopport for msgs merging with different dims
            How to do it?
                Prerequisites: 1. dim must be either 3 or 6 (with velocity)
                To merge dim-3 msg (mean = [mean3]) and dim-6 msg(mean = [mean6,mean6_2]), we just extend dim-3 msg to dim-6 one (mean: [mean3,0],cov[cov3,0;0,inf]) and merge them
    '''
    # if there is a dim-6 msg..
    if len(msgs) ==1:
        return msgs[0]
    dim6Enable = False
    notOneDim  = False
    dim6Index  = -1

    for key,msg in enumerate(msgs):
        assert isinstance(msg,normalDistributeMsg)
        assert(msg.dim() in (1,3,6))
        if msg.dim() == 6:
            dim6Enable = True
            if dim6Index == -1:
                dim6Index = key
            notOneDim = True
        elif msg.dim() == 3:
            notOneDim = True
        elif  notOneDim:
            raise Exception("one dimension should not be mixed with others")
    dim = 6 if dim6Enable else 3 # 3 and 1 are the same way to use
    if dim ==3 and notOneDim == False:
        dim =1

    if dim == 6:
        firstMsg = msgs.pop(dim6Index)
    else:
        firstMsg = msgs.pop(0)

    invcovs =  np.zeros((dim,dim,len(msgs)+1))
    invcovs[:,:,0] = np.linalg.inv(firstMsg.cov)
    meanMerged = np.dot(invcovs[:,:,0], firstMsg.mean)
    for key,msg in enumerate(msgs):
        if dim == 3 or msg.dim()==6 or dim ==1:
            invcovs[:,:,key+1] = np.linalg.inv(msg.cov)
            meanMerged  =  meanMerged + np.dot(invcovs[:,:,key+1],msg.mean)
        else:
            tempcov  = np.zeros((dim,dim))
            tempcov[0:3,0:3] = msg.cov
            tempcov[3:6,3:6] = np.diag((np.Inf,np.Inf,np.Inf))
            tempmean = np.zeros(np.shape(meanMerged))
            tempmean[0:3] = msg.mean 
            invcovs[:,:,key+1] = np.linalg.inv(tempcov)
            meanMerged  =  meanMerged + np.dot(invcovs[:,:,key+1],tempmean)

    invcovsum  = invcovs.sum(2)
    covMerged  = np.linalg.inv(invcovsum) 
    meanMerged = np.dot(covMerged,meanMerged)
    return normalDistributeMsg('amsg',meanMerged,covMerged)

def otherMsgMerge(msgs):
    '''
        guarantee the first msg is normal distributed
    '''
    assert(isinstance(msgs[0],normalDistributeMsg))
    # using the Alg2


    iterateTime =6
    mean = msgs[0].mean
    cov  = msgs[0].cov
    if(np.isnan(mean).any()):
        print 'a'

    if(np.isnan(cov).any()):
        print 'a'
    
    meanFinal =None

    if mean.size!=3:
        assert(mean.size==6)
        meanFinal =  mean
        covFinal  =  cov
        mean      =  meanFinal[0:3]
        cov       =  covFinal[0:3,0:3]
        msgs[0]   =  normalDistributeMsg('norm',mean,cov)

    N   =[400,500,1000,1000,2000,3000,4000]
    # N   =[100,200,300]
    # logging.debug( 'original mean: ')
    # logging.debug(mean)
    # logging.debug( 'original cov: ')
    # logging.debug(cov)

    for iterateCount  in range(iterateTime):
        samples =  np.random.multivariate_normal(mean,cov,N[iterateCount%3])
        msgNormal = normalDistributeMsg('pdfnormal',mean,cov)
        qsamples = msgNormal.read(samples)
        possibilities =  np.ones((N[iterateCount%3]))
        for index in range(0,len(msgs)):
            readings = msgs[index].read(samples) 
            possibilities = possibilities*readings
        # print 'Computing weights....'
        weights  =  possibilities / qsamples
        if(np.isnan(weights).any()):
            print 'b'
        weights  =  weights / np.sum(weights)

        # print 'reevaluating distribution parameters'
        weights  = weights.reshape(weights.shape[0],1)
        mean = np.sum(weights*samples,0)
        dem  =  1 -  np.sum(weights**2)
        num  = np.zeros(cov.shape) 
        for  i  in range(samples.shape[0]):
            res = samples[i,:]-mean
            num += res*res.reshape(res.shape[0],1)*weights[i]
        if dem == 0:
            if np.sum(num) == 0:
                cov = np.diag((np.ones(num.shape[0],)))
            else:
                raise Exception('Something wrong')
        else:
            cov  =  num /dem
        if(np.isnan(mean).any()):
            print 'a'

        if(np.isnan(cov).any()):
            print 'a'
        
    if (meanFinal is None)==False:
        meanFinal[0:3]  = mean
        covFinal[0:3,0:3] =  cov
        covFinal[0:3,3:6] = 0
        covFinal[3:6,0:3] = 0
        mean = meanFinal
        cov  = covFinal
    # logging.debug(  'new mean: ')
    # logging.debug(mean)
    # logging.debug(  'new cov:  ')
    # logging.debug(cov)
    return normalDistributeMsg('amsg',mean,cov)


def mergeMsgs(msgs,guassianAppoximation =None):
    '''
        merge multiple messages into one normalDistributeMsg
        output: normalDistributeMsg
    '''
    if guassianAppoximation is None:
        guassianAppoximation = False
    assert isinstance(msgs,list),'I just accept list args' 
    assert(len(msgs)!=0)
    
    if(len(msgs) == 1):
        return msgs[0]
    
    if guassianAppoximation == False:
        # first, we depart all msgs into two pats: normalDistributeMsg and other
        msgs.sort()  # sort by its id
        normalMsgs = []
        nonormalStart  = len(msgs)
        for key,msg  in enumerate(msgs):
            if(isinstance(msg,normalDistributeMsg)):
                normalMsgs.append(msg)
            else:
                nonormalStart  = key
                break
        mergedMsg = normalMsgMerge(normalMsgs)
        if (nonormalStart == len(msgs)):
            return mergedMsg
        else:
            for i in range(0,nonormalStart):
                msgs.pop(0)
            msgs.insert(0,mergedMsg)
            return otherMsgMerge(msgs)
    else: # guassianAppoximation enabled 
        normaledMsgs=[]
        unnormaledMsgs=[]
        for key,msg  in enumerate(msgs):
            if(isinstance(msg,normalDistributeMsg)):
                normaledMsgs.append(msg)
            else:
                unnormaledMsgs.append(msg)
        normalmerged = normalMsgMerge(normaledMsgs)
        for msg in unnormaledMsgs:
            position  = normalmerged.mean[0:3]
            normalmerged = normalMsgMerge([normalmerged,msg.approximation(position)]) 
        return normalmerged
            
class cmsg(object):
    def __init__(self,name=None):
        if name is None:
            name='cmsg'
        self.name  = name
        self.id  = 0
    def sum(self,cmsg1):
        pass

    # apply product algorithm
    # variable is some variable to be intergrated, it is an index from 0
    def product(self,variable):
        pass
    
    def read(self,parameters):
        '''
            Read a probability with resatPosect to parameters.
            parameters: the messages's arguments 
        '''
        raise Exception("this function hasn't realized yet")

    def sample(self,sampleSize=1):
        '''
            sampling this message with satPosecific size
            TODO can support only Matrpolise sampling method

        ''' 
        pass

    def __mul__(self,other):
        return self.product(other)
    def __cmp__(self,other):
        return cmp(self.id,other.id)

    def __str__(self):
        str = 'mean: '
        if hasattr(self,'mean'):
            str = str +  self.mean.__str__() + '\n cov:\n'
        if hasattr(self,'cov'):
            str = str +  self.cov.__str__() 
        return str
        
class normalDistributeMsg(cmsg):
    '''
        A normal distribution msg, be careful that it can be unnormalized
        mean and cov should be a np.ndarray
    '''
    def __init__(self,name,mean,cov):
        cmsg.__init__(self,name)
        if isinstance(mean, np.ndarray) is False:
            mean = np.array(mean,dtype=float)
        if isinstance(cov, np.ndarray) is False:
            cov  = np.array(cov,dtype=float)
        self.mean = mean
        self.cov = cov
        if len(self.mean.shape)==0:
            self.mean = self.mean.reshape((1,))
            self.cov  = self.cov.reshape((1,1))
        assert len(self.mean.shape)==1,'size error'
        
        
        self.id = -1
        self.dim()
    
    def read(self,x):
        '''
            get probability read in x;
            it isn't normalized
            TODO this iterate method is naive, change this
        '''
        if type(x)!=np.ndarray:
            x = np.array(x,dtype=float)
        
        if(len(x.shape)==1):
            if(self.mean.shape[0]!=1):
                assert(x.shape[0] == self.mean.shape[0])
        else: assert(x.shape[1] == self.mean.shape[0])

        if(len(x.shape)==1):
            if (self.mean.shape[0]==1):
                sequentialSize = x.shape[0]
            else:
                sequentialSize  = 1
        else: sequentialSize  = x.shape[0]

        readings = []
        if(len(x.shape)!=1):
            for index in range(sequentialSize):
                y = x[index,:]
                readings.append(np.exp(-1.0/2*np.dot(np.dot(y-self.mean,np.linalg.inv(self.cov)),y-self.mean)))
        elif(self.mean.shape[0]==1):
            for index in range(sequentialSize):
                y = x[index]
                readings.append(np.exp(-1.0/2*np.dot(np.dot(y-self.mean,np.linalg.inv(self.cov)),y-self.mean)))
        else:
            readings.append(np.exp(-1.0/2*np.dot(np.dot(x-self.mean,np.linalg.inv(self.cov)),x-self.mean)))
        readings = np.array(readings,dtype=float)
        if(np.sum(readings)<1e-40):
            readings = np.ones(readings.shape)
        return readings

    def readNormalized(self,x):
        if type(x)!=np.ndarray:
            x = np.array(x,dtype=float)
        if x.shape != self.mean.shape:
            raise Exception('the shape of input must match')
        return 1/math.sqrt(2*math.pi*np.linalg.det(self.cov))*np.exp(-1/2*np.dot(np.dot(x-self.mean,np.linalg.inv(self.cov)),x-self.mean))
    
    def dim(self):
        '''
            Return dimension of this distribution
        '''
        covshape = self.cov.shape
        meansize = self.mean.size
        assert covshape==(meansize,meansize),'size error'
        return meansize
    def __sub__(self,other):
        assert(isinstance(other,normalDistributeMsg))
        assert(self.dim == other.dim)
        return normalDistributeMsg('anormal',self.mean-other.mean,self.cov+other.cov)

class vMsg(cmsg):
    def __init__(self,name,satPos,var,varrho,bias=None):
        ''' A v distribution msg, be careful that it can be unnormalized
            name: msg's name
            satPos: satellite's position
            var: measurement variance, caution that is NOT sigma, it is sigma^2 instead
            varrho: distance measurement between satellite and agent
            mean should be an array(np.ndarray) and var should be a number
            bias: pesuedoRange's bias 
        '''
        cmsg.__init__(self,name)
        assert(type(satPos)==np.ndarray  or isinstance(satPos,list) or type(satPos)== int or type(satPos)==float )
        assert(type(var)== int or type(var)==float or type(var)==np.ndarray )
        assert(type(varrho)== int or type(varrho)==float or type(varrho)==np.ndarray or type(varrho)==np.float64 )
        if type(satPos)!=np.ndarray:
            satPos = np.array(satPos,dtype=float)      
        self.mean = satPos
        self.var  = var
        self.varrho = varrho
        self.bias = bias
    
    def read(self,p,bias=None):
        '''
            Return an unnormalized probability with resatPosect to p and bias
            TODO some wrong with the return's shape
        '''
        assert(type(p)==np.ndarray or isinstance(p,list) or type(p)== int or type(p)==float ); assert(type(bias)== int or type(bias)==float or isinstance(bias,list) or bias is None)
        if type(p)!=np.ndarray:
            p = np.array(p,dtype=float)
        
        if(len(p.shape)==1):
            if(self.mean.shape[0]!=1):
                assert(p.shape[0] == self.mean.shape[0])
        else: assert(p.shape[1] == self.mean.shape[0])

        if(len(p.shape)==1):
            if (self.mean.shape[0]==1):
                sequentialSize = p.shape[0]
            else:
                sequentialSize  = 1
        else: sequentialSize  = p.shape[0]
        
        # TODO add bias data
        if(bias is None ):
            bias = 0

        readings = []
        if(len(p.shape)!=1):
            for index in range(sequentialSize):
                y = p[index,:]
                readings.append(np.exp(-1.0/(2*self.var)*(np.linalg.norm(y-self.mean)+bias-self.varrho)**2))
        elif(self.mean.shape[0]==1):
            for index in range(sequentialSize):
                y = p[index]
                readings.append(np.exp(-1.0/(2*self.var)*(np.linalg.norm(y-self.mean)+bias-self.varrho)**2))
        else:
            readings.append(np.exp(-1.0/(2*self.var)*(np.linalg.norm(p-self.mean)+bias-self.varrho)**2))
        
        returnItem  =np.array(readings,dtype=float)
        return returnItem.reshape(returnItem.shape[0],)

        # return np.exp(-1/(2*self.var)*(np.linalg.norm(p-self.mean)+bias-self.varrho)**2)

    def dim(self):
        '''
            Return dimension of this distribution
        '''
        return self.mean.size
    
class chiMsg(cmsg):
    '''
        a chi msg class, be careful that it can be unnormalized
        name: Msg's name
        mean: 
        cov: covariance 
        varrho:
    ''' 
    def __init__(self,name,mean,cov,varrho):
        cmsg.__init__(self,name)
        assert(type(mean)==np.ndarray or isinstance(mean,list) or type(mean)== int or type(mean)==float )
        assert(type(cov)==np.ndarray or isinstance(cov,list) or type(cov)== int or type(cov)==float )
        if type(mean)!=np.ndarray:
            mean = np.array(mean,dtype=float)
        if type(cov)!=np.ndarray:
            cov = np.array(cov,dtype=float)
        self.name= name
        self.mean= mean
        if len(self.mean.shape) >1:
            self.mean= self.mean.reshape(self.mean.size,)
        self.cov = cov
        self.varrho = varrho
    def read(self,p):
        assert(type(p)==np.ndarray or isinstance(p,list) or type(p)== int or type(p)==float )
        if type(p)!=np.ndarray:
            p = np.array(p,dtype=float)

        if(len(p.shape)==1):
            if(self.mean.shape[0]!=1):
                assert(p.shape[0] == self.mean.shape[0])
        else: assert(p.shape[1] == self.mean.shape[0])

        if(len(p.shape)==1):
            if (self.mean.shape[0]==1):
                sequentialSize = p.shape[0]
            else:
                sequentialSize  = 1
        else: sequentialSize  = p.shape[0]
        
        readings = []
        if(len(p.shape)!=1):
            for index in range(sequentialSize):
                y = p[index,:]

                x = y - self.mean
                rvector = self.varrho/np.linalg.norm(x)*x
                readings.append(np.exp(-1.0/2*np.dot(np.dot(x-rvector,np.linalg.inv(self.cov)),x-rvector)))
        elif(self.mean.shape[0]==1):
            for index in range(sequentialSize):
                y = p[index]
                x = y - self.mean
                rvector = self.varrho/np.linalg.norm(x)*x
                readings.append(np.exp(-1.0/2*np.dot(np.dot(x-rvector,np.linalg.inv(self.cov)),x-rvector)))
        else:
            x = p - self.mean
            rvector = self.varrho/np.linalg.norm(x)*x
            readings.append(np.exp(-1.0/2*np.dot(np.dot(x-rvector,np.linalg.inv(self.cov)),x-rvector)))
        readings = np.array(readings,dtype=float)
        if(np.sum(readings)<1e-20):
            logging.info('something can be wrong:%0.5e'%(np.sum(readings)))
            if np.sum(readings) != 0:
                while np.sum(readings)< 1e-20:
                    readings = readings*1e10
            else:
                readings = np.ones(readings.shape)
            logging.info('the new reading sum:%0.5e'%(np.sum(readings)))
        return readings
    
    def dim(self):
        '''
            Return dimension of  this distribution
        '''
        return self.mean.size
    def approximation(self,position):
        '''
            guassian approximation given position 
            NOTE only 3d are supported
        '''
        vector = position - self.mean
        
        q          = vector/np.linalg.norm(vector)
        assert vector.shape[0] in (2,3) , "dimension error"
        # signx      = np.sign(vector[2])*2-1
        angleprime = comLibs.direction(position,self.mean)
        
        if vector.shape[0] == 3 :
            R          = comLibs.rotationMatrix(angleprime[0],angleprime[1])
            Rinverse   = comLibs.rotationMatrix(angleprime[0],angleprime[1],True)
        else:
            R          = comLibs.rotationMatrix(angleprime)
            Rinverse   = comLibs.rotationMatrix(angleprime,inverse=True)

        Sigma2PrimeInverse = np.zeros(self.cov.shape)
        Sigma2PrimeInverse[0,0] = np.dot(q,np.dot(np.linalg.inv(self.cov),q))
        Sigma2PrimeInverse[1,1] = 1e-11
        if  vector.shape[0] ==3:
            Sigma2PrimeInverse[2,2] = 1e-11  # NOTE it is tricky to set this value
        mu2prime  = np.zeros(self.mean.shape)
        mu2prime[0]  = self.varrho

        mean   = np.dot(R,mu2prime)+self.mean
        covInverse  = np.dot(np.transpose(Rinverse),np.dot(Sigma2PrimeInverse,Rinverse))
        return normalDistributeMsg('approximationMSg',mean,np.linalg.inv(covInverse))

    def __str__(self):
        str = 'type:chiMsg'
        str += cmsg.__str__(self)
        str += '\n varrho: '+self.varrho.__str__()
        return str
class uniformDistMsg(cmsg):
    '''
        uniformly distributed msg
    '''
    def read(self,p):
        return 1.0

class hybridMsg(cmsg):
    '''
        the hybrid Msg combines with multiple Messages
    '''
    def __init__(self,name=None,msgs=None):
        if name is None:
            name = 'hybridMsg'
        cmsg.__init__(self,name)
        if msgs is None:
            msgs = list([])
        if isinstance(msgs,list):
            self.msgs = msgs
        else:
            self.msgs = [msgs]
    
    def addMsgs(self,msgs):
        if isinstance(msgs,list):
            self.msgs+=msgs
        else:
            self.msgs.append(msgs)
    
    def read(self,x,paras=None):
        '''
            Read the possibilities
        '''
        psTotal = None
        for msg in self.msgs:
            possibilities =  msg.read(x)
            if psTotal is None:
                psTotal = possibilities
            else:
                psTotal =psTotal * possibilities
        return psTotal



if __name__=='__main__':
    # import matplotlib.pyplot  as plt
    np.set_printoptions(precision=3)
    # a = normalDistributeMsg('n1',np.array([1,2]),1*np.eye(2))
    # logging.debug(a.read([[0.1,0.2],[1,0.3],[1,2],[2,3]]))
    # # logging.debug(a.readNormalized([1]))
    # a.dim()
    # # TODO verify the correctness of the reading

    # b = vMsg('v1',[1,2],10,100)
    # logging.debug(b.read([[100,2],[100,2.5]]))

    # c = chiMsg('chi1',[1,2],[[1,0.1],[0.1,1]],10)
    # logging.debug(c.read([[1,1],[1,3],[1,2.5]]))

    # # verify the distribution of each msg
    
    #Normalize

    # a = normalDistributeMsg('n1',np.array([1,2]),np.array([[1,0.5],[0.5,1]]))
    # x = np.linspace(0,3,200)
    # y = np.linspace(1,4,200)

    # xv,yv = np.meshgrid(x,y)
    # ps =np.zeros([200,200])
    # index =0 
    # for ytmp in y:
    #     cordins = np.zeros([200,2])
    #     cordins[:,0] = x
    #     cordins[:,1] = (ytmp*np.ones([200,1])).reshape((200,))
    #     ps[:,index]  =  a.read(cordins)
    #     index  +=1
    
    # h = plt.contourf(x,y,ps)
    # plt.axis('equal')
    # plt.show()
    
    a = normalDistributeMsg('n1',np.array([0,0]),np.diag((1,1)))
    # b = normalDistributeMsg('n1',np.array([3,2,1]),1*np.eye(3))
    # d = chiMsg('a',np.array([-1e7,0,0]),np.diag((1,1,1)),1e7+1)
    d = chiMsg('a',np.array([np.sqrt(2) *0.5 * 1e7,np.sqrt(2)*0.5*1e7]),np.array(((1,0.7),(0.7,1))),1e7)
    # # e = normalDistributeMsg('b',np.array((0,0,0)),np.diag((1,1,1)))
    # c = mergeMsgs([a,b,d])
    # e = mergeMsgs([a,b,d],True)
    print d.approximation(a.mean)
    # print c 
    # print e
    # print np.trace(d.approximation(np.array((0,0,0))).cov)
    # print np.trace(d.approximation(np.array((1,1,0))).cov)
    # print np.trace(d.approximation(np.array((3,1,0))).cov)
    # f =mergeMsgs([d,e])
    # print f.cov
    # print f.mean 
    # g = mergeMsgs([d,e],True)
    # print g.cov
    # print g.mean 
    # print c.cov
    # print c.mean
    # c = mergeMsgs([a,b])
    # print c.cov
    # print c.mean
    # print (a -b).mean
    # print (a -b).cov
    # cov = np.array([[1,0],[0,1]])
    # theta = np.pi*0.0
    # R   = np.array([[np.cos(theta),-np.sin(theta)],[np.sin(theta),np.cos(theta)]])
    # rotatedCov = np.dot(np.dot(R.transpose(),cov),R)


    # c = chiMsg('chi1',[0,4],rotatedCov,6)
    # d = chiMsg('chi2',[0,-4],rotatedCov,6)
    # b = hybridMsg('msg',[a,c,d])

    # x = np.linspace(-10,10,200)
    # y = np.linspace(-10,10,200)

    # xv,yv = np.meshgrid(x,y)
    # ps =np.zeros([200,200])
    # index =0 
    # for ytmp in y:
    #     cordins = np.zeros([200,2])
    #     cordins[:,0] = x
    #     cordins[:,1] = (ytmp*np.ones([200,1])).reshape((200,))
    #     ps[:,index]  =  b.read(cordins)
    #     index  +=1
    
    # h2 = plt.contourf(x,y,np.log(ps),10)
    # plt.axis('equal')
    # plt.show()




    # np.random.seed(123)
    # iter = 1000
    # means = np.zeros((3,))
    # residuals = 0
    # meansPosterior = np.zeros((3,))
    # residualsPosterior = 0

    # sat1TruePos = np.array((1e7,0,0),dtype=float)
    # sat2TruePos = np.array((0.0,0.0,0.0),dtype=float)
    # sat1posCov   = np.diag((0.00001,0.00001,0.00001))
    # sat2posCov   = np.diag((100.0,0.000001,0.000001))
    # for i in range(0,iter): 
    #     dismeasure  = np.linalg.norm(sat1TruePos-sat2TruePos)+1e-2*np.random.randn()
    #     sat1Posmean = np.random.multivariate_normal(sat1TruePos,sat1posCov)
    #     sat2Posmean = np.random.multivariate_normal(sat2TruePos,sat2posCov)

    #     sat1 = normalDistributeMsg('sat1',sat1Posmean,sat1posCov)
    #     sat2 = normalDistributeMsg('sat2',sat2Posmean,sat2posCov)
    #     measurementMsg = chiMsg('chi',sat1Posmean,sat1posCov+1e-8*np.eye(3),dismeasure,sat1posCov,1e-8)

    #     sat2Posterior  = mergeMsgs([sat2,measurementMsg],True)
    #     means  =  means + sat2.mean

    #     residuals =residuals+ np.linalg.norm(sat2.mean-sat2TruePos)**2
    #     meansPosterior  =  meansPosterior + sat2Posterior.mean
    #     residualsPosterior = residualsPosterior+ np.linalg.norm(sat2Posterior.mean-sat2TruePos)**2
    #     # print sat2.mean
    #     # print sat2Posterior.mean
    #     # print i

    # means = means/iter
    # meansPosterior = meansPosterior/iter
    # rms   = np.sqrt(residuals/iter)
    # rmsPosterior   = np.sqrt(residualsPosterior/iter)
    # print means,meansPosterior,np.linalg.norm(means-meansPosterior)
    # print rms,rmsPosterior

    # print sat2Posterior.cov


    ## Test for rotation matrix
    # p1 = np.array((0,0,0))
    # p2 = np.array((1,1,1))
    # angleprime = comLibs.direction(p2,p1)
    # R          = comLibs.rotationMatrix(angleprime[0],angleprime[1])
    # Rinverse   = comLibs.rotationMatrix(angleprime[0],angleprime[1],True)
    # print angleprime,R,Rinverse
    # print np.dot(R,Rinverse)